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Estimation  of  Large,  Dynamic  Processes 
(With  Application  to  Ground  Traffic  Control) 
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-Estimation  “kernels”  form  the  plant 

-Control  algorithms  determine  sequencing  of  operations 
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locations  of  key  vehicles 


Problem: 

Ground  Traffic  Control  Has  Inhomogeneous 

Background 
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Dynamic  Estimation: 

Tracking  Is  Part  of  Ground  Traffic  Control 


-p(x(t1),  x(t2), x(tn)  I  yft),  y(t2) . y(tn)) 


Dynamic  Estimation: 

Tracking  Uses  a  Sequence  of  Random  Variables 
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Also  known  as  Bayes’  nets  (although  this  talk  makes  explicit  the  conditional  pdfs 
on  arcs) 


Dynamic  Estimation: 

Igorithms  Propagate  Information 
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Update:  p(x(tn)  I  y(tn), ...)  =  P(y(tn)  I  x(t„))  p(x(tn)  I ...)  /p(y(tn)) 

Predict:  p(x(tn)  I  y(tn.-,), ...)  =  2{p(x(tn)  I  x(tn..,))  p(x(tn..,)  I  y(Vi)>  ■•■)} 


pfxft)  I  yft))  p(x(y  I  y(t,),y(t2))  P(x(ta)  I  y^), ...)  P(x(t4)  I  y(t4) 


Dynamic  Estimation: 
Smoothing  Algorithms  Propagate  More 

Information 
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Dynamic  Estimation: 
Algorithm  Handles  Out-Of-Sequence 

Measurements 


■  Dynamic  Estimation: 

Graph  Models  Provide  a  Scalable  Scalable 
_ Approach _ 

Each  node  represents  a  random  variable: 

•  Stores  various  conditional  probability  distributions 
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-Bayes  or  C-K*  if  information  flow  opposes  direction  of  link 


Dynamic  Estimation: 

Graph  Algorithms  Provide  a  Scalable  Approach 
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Spatial  Estimation: 

Roads  Strongly  Influence  Ground  Vehicle  Motion 


Spatial  Estimation: 

Road  Shape  Estimation  Is  Part  of  Ground  Traffic 

Control 
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Smoothing  problem: 
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Spatial  Estimation: 
Graphs  Models  Form  Networks 
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General  Graph  Updates  Become  Complex 
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Topological  Algorithms  Restore  Acyclic 

Structure 
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Approximate  Algorithms  Limit  Propagation 
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Spatial  Estimation: 

Approximate  Algorithms  Enhance  Scalability 
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Dynamic  Spatial  Estimation: 

First  Vehicle  Appears,  Gets  Observed 
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Dynamic  Spatial  Estimation: 
Second  Vehicle  Appears,  Gets  Observed 
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Dynamic  Spatial  Estimation: 

The  Update  Process  Dynamically  Maintains 

Consistency 
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Dynamic  Spatial  Estimation: 
Dynamics  Differ  Among  Classes  Of  Nodes 
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Dynamic  Spatial  Estimation: 
Time-scale  Decomposition  Is  Feasible 
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Hypothesis: 

Dynamic  Graph  Algorithms  Need  Control 
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Automatic  Path  Following  and  Video/  Sonar  Data  Acquisition 


Robotic  Air  Vehicles 


Path  to  be  Followed 
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Navigation  System  Design  Using 
Time- Varying  Complementary  Filters 

A.  Pascoal^  I.  Kaminer*  P.  Oliveira  t 


X  Department  of  Aeronautics  and  Astronautics 
Naval  Postgraduate  School 
Monterey  CA 


f  Institute  for  Systems  and  Robotics 
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Department  of  Electrical  Engineering 
Instituto  Superior  Tecnico 
Lisboa,  Portugal 


Summary 


Practical  Problem:  To  develop  advanced  navigation  systems  for 
autonomous  vehicles  (air  vehicles,  oceanographic  surface  craft,  un¬ 
derwater  vehicles). 

Problem  Addressed:  To  estimate  the  velocity  and  position  of 
an  oceanographic  surface  craft  based  on  measurements  provided  by 
a  Doppler  Log  and  a  DGPS  (differential  Global  Positioning  System) 
unit. 

Traditionally,  time-varying  navigation  system  design  is  done  using 
Kalman-Bucy  Filtering  Theory. 

•  Requires  a  complete  stochastic  characterization  of  process  and 

observation  noises  -  a  task  that  may  be  difficult,  costly,  or  not 
suited  to  the  problem  at  hand  [Brown].  ^ 

•  The  filters  lack  stability  and  performance  guarantees 

•  The  filter  performance  characterization  is  not  compatible  with 
conventional  control  system  design  techniques. 
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Summary 


Alternative  approach:  extend  well-known  linear  time-invariant  com¬ 
plementary  filtering  techniques  to  the  time- varying  setting. 

Main  Thrust  of  Theoretical  Research:  To  develop  navigation 
system  design  tools  that  explicitly  address  ”  frequency-like”  perfor¬ 
mance  specifications.  This  allows  tuning  the  characteristics  of  the 
navigation  system  to  the  bandwidths  of  the  sensors  used. 

Theoretical  Difficulty:  The  systems  under  study  are  time-varying. 

Main  Contributions: 

t 

« 

•  ANALYSIS  -  assessment  of  the  performance  of  time- varying  nav¬ 
igation  filters  in  a  frequency  setting. 

•  SYNTHESIS  -  development  of  a  new  methodology  for  the  design 
of  time-varying  navigation  systems  that  takes  explicitly  into  ac¬ 
count  ”  frequency-like”  design  criteria.  Theoretical  tools:  linear 
differential  inclusions  and  linear  matrix  inequalities  (LMIs). 


Outline 


•  Complementary  filtering:  basic  concepts 

•  Time- varying  systems:  mathematical  background 

•  Navigation  system  design:  problem  formulation 

•  Time-varying  complementary  filters:  main  results 


•  Conclusions 


Complementary  Filters:  Basic  Concepts 


Consider 


Problem:  estimate  the  heading  ip  of  a  vehicle  based  on  measure¬ 
ments  rm  and  ipm  of  r  =  ip  and  ip  respectively,  provided  by  a  rate 
gyro  and  a  fluxgate  compass.  The  measurements  are  corrupted 
by  disturbances  and  ipd- 
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Complementary  Filters:  Basic  Concepts 


Clearly, 

1p(s)  =  i^(s)V>(s)  +  Fr(s)r(s), 

where  F^,(s)  =  Ti(s)  =  k/{s  +  k )  and  Fr(s)  =  l/(s  +  &).  This 
suggests  a  filter  with  the  structure 

A 

Ip  =  J~ ifj'lpm  "h  J~rTm 
that  admits  the  state  space  realization 

=  -kip  +  kipm  +  rm  =  rm  +  k(ipm  -  ip) 


jU7 

- - 

■WHS 

n 

1 

) - — ^ 

p  Cx^r 

m 

* 

t 

£.~. 

Fitor  -  F  S 
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Complementary  Filters:  Basic  Concepts 


Simple  computations  show  that 

'fp  =  (7i  ■+■  72)^  +  J'tp'lpd  +  3~rTd-> 

Notice  the  following  important  properties: 

•  Ti(s)  is  low-pass:  the  filter  relies  on  the  information  provided  by 
the  compass  at  low  frequency  only. 

•  T2(s)  =  I  —  Ti(s):  the  filter  blends  the  information  provided 
by  the  compass  in  the  low  frequency  region  with  that  available 
from  the  rate  gyro  in  the  complementary  region. 

•  the  break  frequency  is  simply  determined  by  the  choice  of  the 

1 

parameter  k. 

The  frequency  decomposition  induced  by  the  complementary  filter 
structure  holds  the  key  to  its  practical  success,  since  it  mimicks  the 
natural  frequency  decomposition  induced  by  the  physical  nature 
of  the  sensors  themselves: 

•  the  compasses  provides  reliable  information  at  low  frequency 
only,  whereas 

•  rate  gyros  exhibit  biases  and  drift  phenomena  in  the  same  fre¬ 
quency  region  and  are  therefore  useful  at  higher  frequencies. 
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Complementary  Filters:  Basic  Concepts 


Complementary  filter  design  is  reduced  to  the  computation  of 
the  gain  k  to  meet  a  target  break  frequency  that  is  dictated  by 
the  physical  characteristics  of  the  sensors. 

The  emphasis  is  shifted  from  a  stochastic  framework 
to  a  deterministic  framework  that  aims  at  shaping 
the  filter  closed-loop  transfer  functions. 

Once  this  set-up  is  adopted  -  one  is  free  to  adopt  any  efficient 
design  method  (e.g.H2  or  #00).  The  design  parameters  are  sim¬ 
ply  viewed  as  ’’tuning  knobs”  to  shape  the  charateristics  of  the 
closed  loop  operators.  Filter  analysis  is  easily  carried  out  in  the 
frequency  domain  using  Bode  plots. 

In  the  simple  case  described  here,  the  underlying  process  model 
is 

r 

ip  =  rm-rd 
'Ipm  —  'Ip  'Ipd 

where  r&  and  'ipd  are  process  and  measurement  disturbances. 
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Complementary  Filters:  Basic  Concepts 


Additional  requirement:  design  a  filter  to  reject  the  rate 
gyro  bias. 

Solution:  augment  the  complementary  filter  with  an  integra¬ 
tor. 


Filter  Realization: 

x  =  Ax  +  Bu  -F  H(y  —  y) 
V  =  Cyi 


where  x  =  [x\x^ \  u  =  rm,  y  =  and 


A  = 


0  1 
0  0 


B  = 


1  0 


,H  = 


h 

k2 
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Complementary  Filters:  Basic  Concepts 


Simple  computations  show 


ip  =  {Ti  +  Ti)ip  +  v 


where 


Ti(s)  = 


kiS  +  k2  —  — 


s2  +  k\s  +  k2 


s 2  4-  fas  +  k2 


and 


rj  =  T^d  +  Trrd 


is  a  noise  term,  the  intensity  of  which  depends  on 
FM  =  Us)  and  Fr(s)  =  fi2  ~  *  ~ 


Again,  notice  that 


Us)  +  T2(s)  =  I 


Ti(s)  is  low  pass,  and  T2(s)  is  high-pass. 
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Complementary  Filters:  Basic  Concepts 


The  filter 

—  blends  naturally  the  information  provided  by  the  compass  at 
low  frequency  with  that  available  from  the  rate  gyro  in  the 
complementary  frequency  range 

—  any  constant  term  in  (rate  gyro  bias)  is  rejected  at  the 
output  since  Fr(0)  =  0. 

—  rejects  naturally  high  frequency  noise  present  in  the  fluxgate 
mesurements 
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Complementary  Filters:  Basic  Concepts 


Deterministic  Framework  (r<*  =  =  0)- 

Definition,  Complementary  Filter.  Consider  the 

process  model 


ip  =  r 

% Pm  “  V* 


and  a  filter  T  with  realization 

x  =  Ax  +  Brrm  +  B^ipm 

i  * 

7p  —  Cx 

Then,  T  is  said  to  be  a  complementary  filter  for  M^r  if 

•  F  it  is  internally  stable 

•  For  every  any  initial  conditions  ^(0)  and  x(0)  lirrit-^oo^if)  ~~ 


=  0. 


A 

•  T  satisfies  a  bias  rejection  property,  that  is,  limt^O0/ip  =  0  when 
'ipm  =  0  and  rm  is  an  arbitrary  constant. 

•  The  operator  :  rl>m  is  a  finite  bandwith  low  pass  filter. 
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Time- varying  systems:  mathematical  background 


In  preparation  for  what  follows:  need  to  introduce  the  concept  of 
Finite  Bandwith  Low  Pass  Time- Varying  filters. 

A  causal  system  Q  is  ( finite  —  gain)  stable  if  the  induced  operator 
norm  (maximum  energy  amplification) 

||£7||  :=  sup{I^jh  :  /  € 

is  finite. 

We  will  deal  with  linear  time-varying  systems  with  realizations 

0  :=  Co{{Ai,  B\,  Ci,  Di}, { A z,,  Bl,  Cl ,  DL}} 

where 

Co S  :=  {  E  S,  Ai  4- ...  +  Xl  =  1} 

i—1 

is  the  convex  hull  of  the  set  S  :=  {Ai, An}.  These  are  polytopic 
differential  inclusions. 
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Time- varying  systems:  mathematical  background 


It  can  be  shown  that  given  a  polytopic  system  Q,  |  \Q\  |  <  7  if  3  P  >  0 
such  that 


AfP  +  PAi  PBi  Cf 
BjP  -7 2J  Df 

a  Di  -i 


<  0;i  =  1,2, L. 


Checking  that  such  a  P  exists  can  be  done  using  highly 
efficient  numerical  algorithms  [Boyd  et  ai,  MatLab  Toolbox]. 


Definition.  Low  pass  property.  Let  Q  be  a  linear,  internally 
stable  time-varying  system  and  let  be  a  low-pass,  linear  time- 
invariant  Chebyschev  filter  of  order  n  and  cutoff  frequency  u.  The 
system  Q  is  said  to  satisfy  a  low  pass  property  with  indices  (e,  n) 
over  [0,  coc]  if 

\\(G  -  T)WZ\\  <  e 
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Time-varying  systems:  mathematical  background 


Definition.  Low  pass  filter  with  bandwith  uc.  A  linear, 
internally  stable  time- varying  system  Q  is  said  to  be  an  (e,  n)  low 
pass  filter  with  bandwidth  uc  if 

•  lim^-K)  \\{Q  —  I)  W^||  is  well  defined  and  equals  0. 

•  ujc  :=  sup{a>  :  ||(5  —  /)W”||  <  e},  i.e.  Q  satisfies  a  low  pass 
property  with  indices  (e,  n)  over  [0,  oJ\  for  all  co  G  [0,  uc)  but  fails 
to  satisy  that  property  whenever  u  >  uc. 

•  For  every  5  >  0,  there  exists  to*  =  cj*(S)  such  that  || Q(1  — 
Wff)  ||  <  5  for  u  >  w*. 

The  above  conditions  generalize  the  following  facts  that  are  obvious 
in  the  linear  time-invariant  case: 

•  the  filter  must  provide  a  gain  equal  to  one  at  zero  frequency 

•  there  is  a  finite  band  of  frequencies  over  which  the  system  be¬ 
haviour  replicates  very  closely  that  of  an  identity  operator 

•  the  system  gain  rolls-off  to  zero  at  high  frequency. 
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Navigation  system  design:  problem  formulation 


Basic  Notation 

{X}  is  a  reference  frame;  {B}  is  a  body-fixed  frame  that  moves 
with  the  vehicle. 

—  p  =  [x  y  z]T  -  position  of  the  origin  of  {B}  measured  in 

m- 

—  Jv  =  [x  y  z]T  -  linear  velocity  of  the  origin  of  {B}  measured 
in  {X}. 

—  v  =  [u  v  w]T  -  linear  velocity  of  the  origin  of  {B}  with 
respect  to  {X},  resolved  in  {B} 

—  u>  =  \p  q  r]T  -  angular  velocity  of  {B}  with  respect  to  {X}, 
resolved  in  {B}. 

—  X  =  [(f)  6  'tpY  -  vector  of  roll,  pitch,  and  yaw  angles  that 
parametrize  locally  the  orientation  of  frame  {B}  with  respect 
to  {X}. 
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Navigation  system  design:  problem  formulation 


Rotation  matrix:  #7 Z  (abbreviated  1Z)  is  the  rotation  matrix 
from  {B}  to  {X},  parametrized  locally  by  A,  that  is,  1Z  =  'JZ(X). 

The  following  kinematic  relations  apply 

p  =  7v  =  IZv  and  (2) 

n  =  ns(u),  (3) 


where 


«S(w)  := 


0 

-Uz 

Uy 

Uz 

0. 

« 

—u 

—  LOy 

^ X 

0 

is  a  skew  symmetric  matrix. 


(4) 


Problem:  Estimate  the  inertial  position  p  and  the  velocity  Jv 
of  an  autonomous  vehicle  based  on  measurements  pm  (available 
from  a  DGPS  unit)  and  vm  (available  from  an  onboard  Doppler 
log)  of  p  and  v,  respectively. 
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Navigation  system  design:  problem  formulation 


Constraints: 


—  Due  to  the  physical  characteristic  of  the  Doppler  log,  the 
measurement  vm  is  naturally  expressed  in  body-axis  {B}. 
Furthermore,  Doppler  bias  effects  effects  are  also  naturally 
expressed  in  {B}. 


—  However,  the  measurements  pm  are  directly  available  in  the 
reference  frame  {X}. 
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Navigation  system  design:  problem  formulation 


Definition.  Process  Model  Mp v-  The  time-varying 

process  model  Mpv  is  given  by 


i 

p  =  v 
Pm  =  P 


[  vm  =  iz  xv  +  vdj0 

where  v^ o  is  the  Doppler  bias. 

Assumptions:  the  matrix  IZ  and  its  derivative  7Z  are  constrained 
through  the  inequalities 


i<koi  <  ^rnaxj  l*(t)l  <  6  max  (6) 


and 

|p(t)|  ^  Pmax )  |^(t)|  ^  Qmaxi  Wi^)  I  ^  rmax  CO 
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Navigation  system  design:  problem  formulation 


Definition.  Candidate  complementary  filter.  Consider 
the  process  model  Mpv  with  0  an  arbitrary  constant,  and  let 


T  :=  \ 


(8) 


x  =  A{t)x.  +  Bp(t)pm  +  Bv(t)vm 
p  =  C(t)x. 

Then,  T  is  said  to  be  a  candidate  complementary  filter  for 


Mpv  if 

—  T  is  internally  stable 

—  For  every  initial  conditions  p(0)  and  x(0),  limi_>oo{'0(t)  — 

#)}  =  o. 

—  T  satisfies  a  bias  rejection  property,  that  is,  lim*-^  p  =  0 
when  v  =  0. 


Definition.  Complementary  filter  with  break  fre¬ 
quency  ojc.  Let  J~  be  be  a  candidate  complementary  filter 
for  Mpv ,  and  let  Tv  denote  the  corresponding  operator  from  pm 
to  p.  Then,  T  is  said  to  be  an  (e,  n)  complementary  filter  for 
Mpv  with  break  frequency  uc  if  Tp  is  an  (e,  n)  low  pass  filter 
with  bandwidth  uc. 
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Time- varying  complementary  filters:  main  results 


Problem  formulation.  Given  the  process  model  and 
positive  numbers  uc,  n,  and  e,  find  an  (e,  n)  complementary 
fiter  for  M.pv  with  break  frequency  uc. 

Candidate  complementary  filter  structure 


Theorem:  Consider  the  process  model  Mpv  and  the  time- 
varying  filter 


T:= 


Xi  =  Rvm  +  #x2  +  Ki(p  -  Xi) 
x2  =  R~1K2(  p-xi) 


IP  =  X1 


Suppose  the  filter  T  is  internally  stable.  Then,  T  is  a  candidate 
complementary  filter  for 
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Time-varying  complementary  filters:  main  results 

Sufficient  conditions  for  stability  and  guaranteed  break  frequency 
(using  constant  gains). 

Let 

~  [Pr  Qr  ?V  ] 

such  that 

brl  <Pr,kl  <q?,\rr I  <»•+. 

Then 

i 

wr  €  Co{w‘,i  =  {1,..,8}}  and 
5reCo{5K),i  =  {l,.,8}} 


where 
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Time- varying  complementary  filters:  main  results 


Sufficient  conditions  for  stability  and  guaranteed  break  frequency 


Theorem  Consider  the  linear  time- varying  filter  (9)  and  assume 
that  the  bounds  on  ujr  apply.  Given  n  and  uCi  let 


Aw 

Bw 

Cw 

0 

be  a  minimal  realization  for  a  weighting  Chebyschev  filter  Fur¬ 


ther  let 


F  = 


0  I 
0  Sr 


H  =  [- 1  0] . 
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Time- varying  complementary  filters:  main  results 


Sufficient  conditions  for  stability  and  guaranteed  break  frequency 

Suppose  that  given  e  >  0  3  Me  ft6*3,  Pj  €  ft6*6,  P2  6  ft6*6, 
Pi  >  0,  P2  >  0  such  that  the  linear  matrix  inequalities 


F?Pi  +  HtMt  +  PiFi  +  MH  +  FPH  MCw  +  I^Cw  0 
(MCw  4-  H^Cw)7  P2A  +  ATP2  +  CyyCw  P^Bw 

0  B^P2  -e2I 


<0, 


Fi  = 


0  I 
0  S(<4) 


<  =  {1,-8} 


(10) 


are  satisfied.  Then,  the  constant  gains 

K2 

make  the  filter  T  internally  stable.  Furthermore,  the  operator 
Tv  :  p  ->  p  satisfies  a  low  pass  property  with  indices  (e,  n)  over 
[0, wc],  that  is,  ||(ftp  - 1)  W£c ||  <  e. 

Therefore,  the  problem  is  reduced  to  checking  the  feasibility  of 
the  set  of  linear  matrix  inequalities 
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Time-varying  complementary  filters:  main  results 


Indication  of  Proof 

Given  the  candidate  filter  Tv,  consider  the  Lyapunov  coordinate 
transformation 

CW  = 

where 

I  0  1 

0 


With  this  change  of  coordinates,  the  operator  Tp  admits  the 
realization 


Tp 


'  C  =  ( PAP -1  +  PP-1)  C  +  PBpp 

p  =  cp-K 


Using  the  relations 


PAP"1 


-Ki  I 
-K2  0 


and 

pp-i 


0  0 

o  o’ 

0  0 

o 

0  S{Ku) 

0  S(cur) 
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the  realization  can  be  written  as 

-K,  I 
-K2  S(u>r) 
p  =  V  °]  C 


c + 


Kx 

K2 


P 


(12) 


Simple  algebra  now  shows  that  {Tv  —  I)  W£c  admits  the  state- 
space  representation 


'  -Kx 

I 

KiCw 

* 

0 

-k2 

Sr 

K2Cw 

0 

0 

0 

Aw 

Bw 

I 

0 

—Cw 

0 

(13) 


'  F  +  KH  KCW 

0 

0 

Aw 

Bw 

H 

—Cw 

0 

( 


€  Co  \ 


Fi  +  KH  KCW 

0 

0 

Aw 

Bw 

,i  =  {l,...,8}> 

H 

-Cw 

0 

where 


K  = 


K2 


and  F,  F,  and  Fi  are  defined  as  before.  The  Theorem  follows 
from  the  computation  of  the  induced  operator  norm  of  the  poly¬ 
topic  system  ( Tv  —  I)  W£c. 
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Time- varying  complementary  filters:  main  results 


A  Practical  Algorithm  for  Navigation  System  Design 

Mathematical  tools  were  introduced  to  design  a  candidate  com¬ 
plementary  filter  with  a  guaranteed  break  frequency. 

Notice:  the  outcome  of  the  design  process  may  very  well  be  a 
filter  with  an  effective  bandwidth  that  is  greater  than  the  one 
required. 

The  set  of  possible  solutions  must  be  further  constrained  so 
that  the  designer  have  an  extra  design  parameter  at  his  disposal 
to  select  one  solution  (if  it  exists)  that  meets  the  required  band¬ 
width  criterion. 

Solution  in  the  linear  time-invariant  case:  make  the  fil¬ 
ter  ” roll-off  sufficiently  early  in  frequency”. 
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Time-varying  complementary  filters:  main  results 


A  Practical  Algorithm  for  Navigation  System  Design 

In  the  time- varying  setting:  make  |  \TP{I  —  W™*)  1 1  sufficiently 
small  for  adequate  choices  of  ut  and  nt,  which  play  the  role  of 
’’tuning  parameters”. 

Practical  algorithm:  modify  the  algorithm  described  to 
include  a  new  ’’high-frequency”  constraint,  which 
can  be  easily  cast  as  a  Linear  Matrix  Inequality. 

t 

t 

It  is  up  to  the  system  designer  to  select  appropriate  values  of 
the  tuning  parameters  to  try  and  meet  all  the  criteria  that  are 
required  for  the  complementary  filter. 
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Example 


—  Vehicle  progresses  at  2 m/s  with  maximum  yaw  rate  3 rad/ s. 

—  The  Doppler  log  has  a  bias  v^o  =  [0.1  m/s,  0.2  m/s]T. 

—  The  selected  break  frequency  was  uc  —  0.1  rad/ s. 
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Conclusions  8z  Future  Work 


—  A  new  methodology  was  developed  to  design  linear  time- 
varying  complementary  filters  in  a  frequency  set¬ 
ting. 

—  The  problem  of  filter  design  was  cast  in  the  framework  of 
linear  differential  inclusions  (polytopic  systems). 

—  The  design  method  involves  determining  the  feasibility  of  a 
set  of  linear  matrix  inequalties 

—  FUTURE  WORK:  extend  the  results  to  the  discrete-time, 
multi-rate  case  by  exploring  the  well  known  isomorphism 
betwee  multi-rate  and  invariant  systems. 
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Integrated  Design  of  Guidance  and  Control  Systems  for  AUVs 
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•Several  papers  by  Claude  Samson  (INRIA,  France)  et  al  on  tracking 
and  path  following  for  land  vehicles.  The  methods  are  developed  for  the 
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Kinematic  equations  in  the  Serret-Frenet  frame 
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accounting  for  the  roll  angle 


set  by  the  vehicle  dynamics. 
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the  propulsion  force  is  chosen 
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Backstepping  kinematics  into  dynamics  -  first  step 
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Backstepping  kinematics  into  dynamics  -  second  step 
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Problem  Statement 


Plant  Controller  Optimization  Problem  (PCO) 

Given  an  AUV  -  with  a  fixed  baseline  body  configuration  -  that 

is  required  to  operate  over  a  finite  set  of  representative  trimming 

conditions  in  the  vertical  plane,  determine  the  optimal  size  of  the 

bow  and  stem  control  surfaces  so  that  a  weighted  average  of  the 

power  required  at  the  trimming  conditions  is  minimized,  subject  to 

«  * 

the  conditions  that: 

i)  open  loop  requirements  are  met  and 

ii)  stabilizing  feedback  controllers  can  be  designed  to  meet  time 
and  frequency  closed  loop  performance  requirements  about  each 
trimming  point. 
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Open  /  closed  loop  requirements 


Open  loop  requirements. 

-  Possibility  of  achieving  trim  at  each  operating  condition. 

-  Meeting  a  desired  degree  of  open-loop  stability. 

Closed  loop  requirements. 

-  Maneuverability  specifications  in  response  to  depth  commands. 

-  Hard  limits  on  surface  deflections. 

-  Actuator  bandwidth  constraints. 


Mission  example:  three  phases 


Figure  1:  A  typical  three  phase  vehicle  mission. 


The  50  minute  mission  has  three  distinct  phases: 

t  * 

-  phase  1,  duration  15  minutes:  dive  to  233  m  depth  with  velocity 
vtQ  =  1.0  m/s  and  flight  path  angle  70  =  -15  deg. 

-  phase  2,  duration  20  minutes:  perform  a  survey  over  a  3000  m 
stretch  above  the  sea  bed  while  on  straight  level  flight  at  cruise 
speed  2.5  m/s  h 

-  phase  3,  duration  15  minutes:  re-surface  with  velocity  of  vtQ  = 
1.0  m/s  at  flight  path  angle  70  =  -15  deg. 
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Key  Ideas  /  Theoretical  Tools 


Key  idea.  Cast  the  PCO  problem  in  the  form  of  a  new  constrained 
optimization  problem  where 

-  the  cost  J  to  be  minimized  is  the  average  power  required  at  trim¬ 
ming. 

-  the  search  is  done  over  theset  of  feedback  controllers  that  meet 
open  loop  and  closed  loop  requirements. 

Facts  The  cost  J  can  be  written  explicitly  in  terms  of  the  vehicle 

t 

surface  sizes.  The  open  and  closed  loop  requirements  can  be  expressed 
as  Linear  Matrix  Inequalities  (LMIs)  that  are  also  functions  of  the 
control  surface  sizes. 

Optimization  problem  The  PCO  problem  is  reduced  to  minimizing 
a  certain  function  of  the  surface  sizes,  while  satisfying  a  finite  set  of 
LMI  constraints. 

Tools  The  new  problem  is  solved  numerically  by  resorting  to  efficient 
convex  optimization  algorithms  /  LMI  Toolbox. 
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Vehicle  Dynamic  Model 


Basic  Notation. 

{B}  -  body  fixed  frame;  {/}  -  reference  frame, 
p  =  z]'  -  position  of  the  origin  of  {B}  measured  in  {/}  ; 

v  =  [it,  w]'  -  body-fixed  linear  velocity; 

9  -  pitch  angle; 

q  -  angular  velocity  of  {B}  relative  to  {/}  ; 

=  [u,  w,  q]f  -  extended  velocity  vector  in  the  vertical  plane; 
5  :=  [£6,  £J'  bow  and  stern  plane  deflections. 


Underwater  vehicle  model. 

mRbAv  +  CW4»)4>  =  q»>  o,  s,  t) 

6  =  q\  z  —  —usinO  +  wcosO 


where 


tv  -  vector  of  external  forces  and  moments, 
Mrbv  -  generalized  inertia  matrix, 

Crbv  -  matrix  of  Coriolis  and  centripetal  terms, 
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Vehicle  Model  Parametrization 


Parameterization  of  the  AUV  vertical  plane  equations  of  motion  in 
terms  of  the  bow  and  stem  control  surface  sizes  £&  and  (s. 
Assumptions 

•  the  cord  c  and  length  d  of  the  control  surfaces  are  such  that  their 
aspect  ratio  AR  =  d/c  is  constant. 

•  the  control  surfaces  have  a  constant  profile. 

•  thrpghout  the  optimization  procedure  the  control  surface  rotation 
axes  are  maintained  at  fixed  positions. 
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Trim  (Equilibrium)  points 


Trim  (equilibrium)  point:  set  of  input  and  state  variables  for 
which  the  net  sum  of  the  forces  and  moments  acting  on  the  vehicle  is 
zero. 


Equilibrium  point  -  formal  definition:  a  vector 

(q,oAAoA>To) 


such  that 

^RBv(quo)qvO  q<;0,  ^0’  ^0’  V  o 


The  only  equilibrium  points  of  the  AUV  in  the  vertical  plane  are  those 
that  correspond  to  straight  line  trajectories ,  parameterized  in  terms 
of  total  speed  vt  and  flight  path  angle  7. 

Due  to  the  existence  of  two  control  surfaces  the  trimming  solution  is 
not  unique,  and  the  additional  constraint  of  setting  the  bow  planes 
deflection  to  zero  at  trimming  is  imposed. 
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Stern  Plane  Deflection  at  Trimming 


The  stern  plane  deflection  at  trimming  can  be  written  as 

^sq  KSs {%  J  VtQ,  CbiCs) 

where  Kss(-)  is  a  nonlinear  function  of  the  trimming  variables  70  and 
vtQ  and  the  control  surface  areas  and  (s. 


Evolution  of  K$s  ()  for  a  trimming  point  characterized  by 
VtQ  —  1.5  m/s  and  70  =  —15  deg 
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Stern  Plane  Deflection  at  Trimming 


The  stern  plane  deflection  at  trimming  can  be  approxi¬ 
mated  by  a  first  order  Taylor  expanson  in  the  variable 

i_. 

&  ‘  o  1  i 

Sso  —  ^<5S( 7o>  ^o»  Cso)  C&o’  Cso) 

Ss 

where  and  Cs0  are  nominal  values  about  which  the  expansion  is 
done. 


Actual  and  approximate  values  of  the  stern  plane  deflection  at  trim¬ 
ming  for  the  case  where  vtQ  =  1.5  m/s  and  70  =  15  deg.  Solid  line: 
approximation  computed  abou  the  nominal  values  <J,0  =  £So  =  0.4  m2. 
Dashed  lines:  actual  function  obtained  for  £&0  €  {0.3, .,  0.6,  0.7}m2. 
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Stern  Plane  Deflection  Constraint 


Using  the  approximation 


*6.  (To  ’  V*0  3  C&o  5  Cso)  “t-  >  (7o  5  Vto 3  C&o  5  Cso  ) 

Ss 


a  constraint  on  the  stern  surface  area  for  a  given  trimming 
point  \Ss\  <  £Smsx  is  locally  written  as 


-^trim(Cs)  Cs(^Qs(7o3  ^0’  C&0’  Cso)  O  "h  ^5s(7 oj  Cfy)3  Cso)  ^  0 

•^trim(Cs)  :=:  Cs(^(5s(7o3  ^0  3  C&0»  Cso)  +  O  “h  ^'Ssi'y 03  C&03  Cso)  ^  0 


These  are  linear  inequalities  in  the  stern  control  surface  area 

Cs. 
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Power  required  at  trimming 


For  small  angles  of  attack,  the  thrust  power  Pt  at  trimming  equals 
Pt  =  Ttvto  where 

Tt  =  Tt{%,vt0,(b,(s )• 
is  the  thrust  required  to  trim  the  vehicle. 

The  figure  shows  the  the  function  Tt  obtained  for  a  trimming  trajectory 
characterized  by  Vt0  =  1.5  m/s  and  70  =  —15  deg 
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The  thrust  function 


Thrust  function  shape:  an  interplay  among  competing  effects: 

i)  when  the  size  of  the  stern  control  surfaces  decreases  they 
must  deflect  considerably  to  achieve  trimming  a  low  speed  and 
high  flight  path  angles.  This  will  increase  the  total  drag  and 
therefore  T. 

ii)  when  the  size  of  the  stern  control  surfaces  increases  the  deflection 

angles  that  are  required  for  vehicle  trimming  decrease;  however, 

« * 

since  the  profile  drag  is  proportional  to  the  surface  area,  the  total 
drag  will  eventually  increase. 

Hi)  when  the  size  of  the  bow  control  surfaces  decreases  the  lift  force 
they  generate  decreases  and  the  stern  control  surfaces  must  deflect 
to  compensate.  This  will  increase  the  total  drag. 

iv )  when  the  size  of  the  bow  control  surfaces  increases  the  profile  drag 
increases  and  the  total  drag  will  eventually  increase. 
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Thrust  Approximation 


The  thrust  Tt  at  trimming  can  be  approximated  as 

^t(%5  Cs) :==  t;0(7o, vto,  c^o’  Cso)  t  r^(7o, Co)a  d- 

7tbl  (%  5  ^0  5  C&o  5  Cso  )  Cft  "b  ^sO  (To  5  ^0  >  C&0  5  Cso  )  Cs  + 
%Sn»  ^0»  C&o5  Cso)C 

by  expanding  Tt  in  series  of  powers  of  £&,“£$, Cb1^  an<^  C"1* 

Figure  compares  the  actual  and  approximate  values  of  Tt 
for  vtQ  =  1.5  m/s  and  70  =  15  deg. 


The  solid  line  represents  the  actual  function  for  £&  =  0.4  in2,  dashed 
lines  represent  the  approximation  for  £So  £  {0.3,  0.4,  0.5,  0.6,  0.7}m2. 
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Average  Propulsion  Power 


Average  propulsion  power  for  a  given  mission: 

J(G»  Cs)  :=ZPiPi( 7$,  <,&<,), 

i 

where  P/(.)  is  the  power  required  to  trim  the  vehicle  at  the  flight  con¬ 
dition  specified  by  7J  and  v\Q  and  Pi  is  the  percentage  of  total  mission 
time  that  is  spent  at  mission  phase  i. 


The  figure  shows  the  average  propulsion  power  versus  the 
control  surface  sizes  obtained  for  the  mission  defined  by 

J(Ct,  0)  =  0.3Pt(— 15, 1,  a,  C)+0.4P(0, 2.5,  a,  Cs)+0.3Pt(15, 1,  (b,  Q 
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Average  Propulsion  Power  versus  Surface  Size 


The  figure  shows  the  evolution  of  the  optimal  bow  and  stern 
surface  sizes  for  depth  changing  maneuvers  executed  with  |70|  =  15° 
and  vtQ  e  [0.5,  2.5]  m/s.  During  level  flight,  vtQ  =  2.5  m/s. 


At  lower  velocities  the  presence  of  bow  planes  allows  the  vehicle  to 
be  trimmed  at  smaller  angles  of  attack,  the  reduction  in-  drag  far  out¬ 
weighing  the  drag  that  is  induced  by  augmenting  their  size. 

At  higher  velocities ,  small  control  surfaces  with  small  deflections  are 
able  to  produce  the  forces  and  torques  required  to  steer  the  vehicle 
along  the  depth  changing  maneuvers. 
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Average  Propulsion  Power  versus  Surface  Size 


Figure  was  obtained  for  a  three  phase  mission,  where  the  parameter 
depth  took  values  in  the  interval  [0,  250]  m.  The  optimal  bow  and 
stern  surface  sizes  were  computed  for  depth  changing  maneuvers 
executed  with  The  figure  shows  the  evolution  of  the  optimal  bow 
and  stern  surface  sizes  for  depth  changing  maneuvers  executed 
with  |70|  =  15°  and  vtQ  6  [0.5,  2.5]  m/s.  During  level  flight,  vtQ  = 
2.5  m/s. 


Small  depths :  trimming  achieved  with  all  control  surfaces  near  to  zero. 
This  solution  minimizes  the  propulsion  power  required  for  the  second 
phase  of  the  mission.  As  depth  increases :  optimal  surface  sizes  increase 
to  compensate  for  the  drag  generated  by  the  control  surface  deflections 
necessary  to  steer  the  vehicle  along  the  depth  changing  maneuvers. 
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Vehicle  Linear  Model 


The  AUV  vertical  plane  model  can  be  written  as 

xu  ~  FyipC-W)  Uv,  £5,  £ s ) 

where  Fv  is  a  nonlinear  function,  xv  =  (a,  g,  0 , 2:/  is  the  state  vector, 
and  uv  =  (<5&,  £5) '  is  the  input  vector. 


The  model  is  linearized  about  a  trimming  point  to  obtain: 

xu  =  A^q,  VtQ,  Cbt  Cs)Xu  "h  5(7o,  ^0’  C&j  Cs)uu 

where  A(.)  and  I?(.)  are  matrices  that  depend  on  the  trimming  point 
(70  and  vto)  and  on  the  control  surface  sizes. 

The  AUV  linear  model  can  be  re-written  as: 

=  [Ao  +  OfeA  1  +  O5A2]  xv  +  [Bo  +  CsbBi  +  Os^2] 

where  A*  =  A*( 70,  C&0>  Go);  =  #t(70>  v*o>  C&0>  C«0)- 

Important  Fact:  the  approximate  linearizations  show  &  linear  de¬ 
pendence  with  the  variables  and  (s. 
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Dynamical  Requirements 


Open  Loop  Regional  Pole  Placement  Constraints 

Open  loop  generalized  stability  region 


Define 

A0i  =  [-A(70,  vto,  Cb,  Oi,j] ,  with  i,j  =  1, 2, 3 

This  constraint  is  satisfied  if  and  only  if  there  exists  a  symmetric  pos¬ 
itive  definite  matrix  XQi  >  0  that  verifies  the  generalized  stability 
Lyapunov  inequality 


Roi{A0i,  Xoi,  a,  0)  <  0 


where  R0i{.)  is  given  by 


Roi(-)  = 


sin {$)(A0lX0i  +  XoiATol) )  cos {6){XolATol  -  AolX0i)  0 

cos (0)(AoiXol  -  XolATol)  sin (e)(AolXol  +  XolATol)  0 

0  0  A0iX0i  +  X0iArf  +  2  aX0i 
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Open  Loop  Eigenvalues 


Evolution  of  the  two  dominant  open  loop  eigenvalues.  Each  curve  was 
obtained  for  a  value  of  G  {0.01,  0.2,  0.4,  0.6,  0.8,  1.0}  m2  and  ( s 
within  the  interval  [0.01 , 1.0]  m2,  for  70  —  0  and  Vt0  =  2.5  m/s. 


Conclusions:  small  impact  of  the  bow  control  surface  size  on  overall 
system  open  loop  stability.  This  gives  the  designer  an  extra  degree 
of  freedom  that  can  be  used  to  improve  the  vehicle  ma¬ 
neuverability. 
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Closed  loop  requirements:  the  Hoo  set-up 


Feedback  interconnection. 


Let  Q  admit  the  realization 

x  =  Ax  +  Bww  +  Buu 
z  =  Cx  -1-  Du 
y  =  x 

Then  ||7^w||oo  <  7  if  and  only  if  there  exist  a  symmetric  positive 
definite  matrix  X  €  3ftnxn  and  a  matrix  W  G  3ft9  xn  such  that  the  linear 
matrix  inequality  (LMI)  Roo{X,  W,  7)  <  0  holds,  where  Roo(X,  W,  7) 
is  defined  by 


AX*,  +  BUW  +  Zoo  AT  +  VTBl 

Bw 

Xoo  CF  +  WTDt 

Bl 

-7 1 

Dt 

CXm  +  DW 

D 

-7 1 

In  case  of  feasibility  a  state  feedback  gain  is  obtained  as  K  =  WY  1 
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Tioo  Design  Constraint 


Closed  Dynamic  Requirements 

•  there  should  exist  a  single  state  feedback  controller  that  simulta¬ 
neously  stabilizes  the  AUV  about  all  trimming  conditions; 

•  zero  steady  state  in  response  to  depth  commands; 

•  minimum  depth  command  bandwidth  of  0.5  rad/s; 

•  the  maximum  bow  and  stern  plane  control  bandwidths  should  not 
exceed  2  rad/s, 

i  1 

Figure  represents  the  constraint  7  <  0.8  boundary  of  allowable 
surface  sizes 
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Numerical  solution:  cost  evolution 


In  the  Figure: 

•  Too,  Closed  loop  requirements. 

•  r+,  upper  limit  of  open  loop  degree  of  stability  requirement. 

•  P",  lower  limit  of  open  loop  degree  of  stability  requirement. 

•  I \rimi  maximum  surface  deflection  at  trimming. 
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Conclusions 


Main  Results 

A  new  methodology  was  introduced  for  the  integrated  design  of 
plant  parameters  and  feedback  controllers  to  meet  AUV  mis¬ 
sion  performance  requirements  with  minimum  energy  expenditure. 

Techniques  used:  Firmly  rooted  on  Linear  Matrix  Inequalities  (LMI) 
theory.  Numerical  solutions  avaiable  with  the  Matlab  LMI  Toolbox. 

Future  work:  Extending  the  technique  to  address  dynamic  require¬ 
ments  in  the  presence  of  wave  disturbances  (operation  at  very  small 
depths). 
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